#pragma once

#include <boost/interprocess/mapped_region.hpp>
#include <boost/interprocess/shared_memory_object.hpp>
#include <boost/interprocess/sync/named_mutex.hpp>
#include <memory>
#include <ros/ros.h>
#include <ros/serialization.h>
#include <utility>

using namespace boost::interprocess;
namespace Serializer = ros::serialization;

using ShareMemory = boost::interprocess::shared_memory_object;
using ShareMemoryRegion = boost::interprocess::mapped_region;
using ShareMemoryMutex = boost::interprocess::named_mutex;

template <typename MessageType>
class SharedMemoryROSCommunicator
{
public:
  explicit SharedMemoryROSCommunicator(std::string channel,
                                       std::size_t memory_size = 1024)
      : channel_(std::move(channel)), memory_size_(memory_size)
  {
    try
    {
      creator_ = false;
      sharedMemoryPtr_ = std::make_unique<ShareMemory>(
          open_only, channel_.c_str(), read_write);
    }
    catch (const boost::interprocess::interprocess_exception &ex)
    {
      creator_ = true;
      sharedMemoryPtr_ = std::make_unique<ShareMemory>(
          open_or_create, channel_.c_str(), read_write);
    }
    sharedMemoryPtr_->truncate(memory_size_);
    regionPtr_ =
        std::make_unique<ShareMemoryRegion>(*sharedMemoryPtr_, read_write);
    std::string mux_name = channel_ + "_mux";
    mutexPtr_ =
        std::make_unique<ShareMemoryMutex>(open_or_create, mux_name.c_str());
  }

  void publish(const MessageType &msg)
  {
    uint32_t buf_size = Serializer::serializationLength(msg);
    boost::shared_array<uint8_t> buffer(new uint8_t[buf_size]);
    Serializer::OStream stream(buffer.get(), buf_size);
    Serializer::serialize(stream, msg);
    mutexPtr_->lock();
    memcpy(regionPtr_->get_address(), buffer.get(), buf_size);
    mutexPtr_->unlock();
  }

  MessageType read()
  {
    MessageType msg;
    boost::shared_array<uint8_t> buffer(new uint8_t[memory_size_]);
    mutexPtr_->lock();
    memcpy(buffer.get(), regionPtr_->get_address(), memory_size_);
    mutexPtr_->unlock();
    Serializer::IStream stream(buffer.get(), memory_size_);
    Serializer::deserialize(stream, msg);

    return msg;
  }

  ~SharedMemoryROSCommunicator()
  {
    if (creator_)
    {
      sharedMemoryPtr_->remove(channel_.c_str());
      std::string mux_name = channel_ + "_mux";
      ShareMemoryMutex::remove(mux_name.c_str());
    }
  }

private:
  std::string channel_;
  std::size_t memory_size_;
  bool creator_;
  std::unique_ptr<ShareMemory> sharedMemoryPtr_;
  std::unique_ptr<ShareMemoryRegion> regionPtr_;
  std::unique_ptr<ShareMemoryMutex> mutexPtr_;
};
